/**
 * @file my_quadword.h
 * @brief 四元数
 * @details 四元数的运算以及旋转相关变量的运算
 * @author all
 * @date 2023/11/7
 * @version 1.0
 * @par Update Record:
 * - 2023/11/3 创建文档
 * - 2023/11/7 添加注释
*/
/********************<start>**********************/
#ifndef MY_QUATERNION_H
#define MY_QUATERNION_H

/********************<include>********************/
#include"my_vector.h"

namespace my_tf
{
/********************<class>**********************/
   /**
    * @brief 四元数类
    * @details 四元数的相关函数
    */
   class Quaternion
   {
    private:
        double m_floats[4];
    public:
        Quaternion() {}
        Quaternion(const double& x, const double& y, const double& z, const double& w) 
	    {
            m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w;
        }
        /**
        * @brief 设置四元数的值
        * @param x x分量
        * @param y y分量
        * @param z z分量
        * @param w w分量
        */
        inline void	setValue(const double& x, const double& y, const double& z,const double& w)
        {
            m_floats[0]=x;
            m_floats[1]=y;
            m_floats[2]=z;
            m_floats[3]=w;
        }
        /**
        * @brief 使用轴角表示法设置旋转
        * @param axis 旋转轴
        * @param angle 旋转角度（弧度）
        */
        void setRotation(const Vector3& axis, const double& angle)
        {
            double d = axis.length();
            myAssert(d != double(0.0));
            double s = tfSin(angle * double(0.5)) / d;
            setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
                tfCos(angle * double(0.5)));
        }
        /**
        * @brief 欧拉角转四元数
        * @param roll 绕x轴的旋转角度（弧度）
        * @param pitch 绕y轴的旋转角度（弧度）
        * @param yaw 绕z轴的旋转角度（弧度）
        */
        void setRPY(const double& roll, const double& pitch, const double& yaw)
        {
            double halfYaw = double(yaw) * double(0.5);  
            double halfPitch = double(pitch) * double(0.5);  
            double halfRoll = double(roll) * double(0.5);  
            double cosYaw = tfCos(halfYaw);
            double sinYaw = tfSin(halfYaw);
            double cosPitch = tfCos(halfPitch);
            double sinPitch = tfSin(halfPitch);
            double cosRoll = tfCos(halfRoll);
            double sinRoll = tfSin(halfRoll);
            setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
                            cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
                            cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
                            cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
        }
        /**
        * @brief 获取旋转轴
        * @return 旋转轴向量
        */
        Vector3 getAxis() const
        {
            double s_squared = double(1.) - tfPow(m_floats[3], double(2.));
            if (s_squared < double(10.) * TFSIMD_EPSILON) //Check for divide by zero
                return Vector3(1.0, 0.0, 0.0);  // Arbitrary
            double s = tfSqrt(s_squared);
            return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
        }
        /**
        * @brief 计算该四元数与另一个四元数的点积
        * @param q 另一个四元数
        * @return 点积结果
        */
        double dot(const Quaternion& q) const
        {
            return m_floats[0] * q[0] + m_floats[1] * q[1] + m_floats[2] * q[2] + m_floats[3] * q[3];
        }
        /**
        * @brief 计算四元数的平方长度
        * @return 平方长度
        */
        double length2() const
        {
            return dot(*this);
        }
        /**
        * @brief 计算四元数的长度
        * @return 长度
        */
        double length() const
        {
            return tfSqrt(length2());
        }
         /**
         * @brief 归一化四元数
         * @details x^2 + y^2 + z^2 +w^2 = 1
         * @return 归一化后的四元数
         */
        Quaternion& normalize() 
        {
            return *this /= length();
        }
         /**
         * @brief 将四元数进行反比例缩放
         * @param s 缩放因子
         * @return 缩放后的四元数
         */
        Quaternion& operator/=(const double& s) 
        {
            myAssert(s != double(0.0));
            return *this *= double(1.0) / s;
        }
        /**
        * @brief 将四元数进行缩放
        * @param s 缩放因子
        * @return 缩放后的四元数
        */
        Quaternion& operator*=(const double& s)
        {
            m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
            return *this;
        }
        inline	operator       double *()       { return &m_floats[0]; }
	    inline	operator const double *() const { return &m_floats[0]; }
        ///@brief 返回x值
        inline const double& x() const { return m_floats[0]; }
        ///@brief 返回y值
        inline const double& y() const { return m_floats[1]; }
        ///@brief 返回z值
        inline const double& z() const { return m_floats[2]; }
        ///@brief 返回w值
        inline const double& w() const { return m_floats[3]; }
   };
}

#endif
/********************<end>********************/